import csv
import json
import math
import os.path
import random
import threading
import time
from datetime import datetime

import redis
from PySide2.QtCore import *
from pyproj import Proj
from shiboken2 import wrapInstance

from TESS_PythonAPI.Tessng import *
from utils.net_uilts import point_in_polygon, haversine, calculate_polygon_center

# 用户插件子类，代表用户自定义与仿真相关的实现逻辑，继承自PyCustomerSimulator
#     多重继承中的父类QObject，在此目的是要能够自定义信号signlRunInfo

car_ids = dict()
car_send_ids = dict()
car_real_type = {0: 0, 1: 1, 2: 2, 3: 3, 4: 4, 5: 5, 6: 6, 7: 7, 8: 8, 9: 1, 10: 10, 11: 11, 15: 15, 16: 16, 17: 17,
                 18: 18}
car_tess_type = {1: 1, 2: 7, 3: 3, 4: 2, 5: 6, 6: 5, 7: 4, 8: 8}
car_color = {0: f"#FFFFFF", 1: f"#FFFFFF", 2: f"#000000", 3: f"#FF0000", 4: f"#C0C0C0", 5: f"#FFFF00", 6: f"#4169E1",
             7: f"#00FF00", 8: f"#802A2A", 9: f"#C0C0C0"}
car_imitate_real_type = {1: 1, 2: 4, 3: 6, 4: 1, 5: 1, 0: 1}
car_imitate_tess_type = {1: 1, 2: 7, 3: 5, 4: 1, 5: 1}


class CustomSimulator(QObject, PyCustomerSimulator):
    signalRunInfo = Signal(str)
    forStopSimu = Signal()
    forReStartSimu = Signal()

    def __init__(self, config_dict: dict):
        QObject.__init__(self)
        PyCustomerSimulator.__init__(self)
        # 当前正在仿真计算的路网名称
        self.mNetPath = None
        # 相同路网连续仿真次数
        self.mSimuCount = 0
        self.config_dict = config_dict
        # 背景车避让距离
        self.avoidDistance = 50
        # 背景车发车轨迹列表
        self.start_car_dict = {}
        # 背景车辆列表
        self.all_car_dict = {}
        # 主车
        self.target_start_car_dict = {}
        # 主车列表
        self.target_all_car_dict = {}
        # 激进车
        self.radical_ratio = config_dict['radical_ratio']
        self.radical_car_list = []
        # 保守车
        # self.conservative_ratio = config_dict['radical_ratio']
        self.conservative_car_list = []
        # 需要控制的车辆
        self.control_dict = dict()
        # 批次号
        self.batchNum = 0
        # 经纬度转化公式
        self.p = Proj(config_dict['proj'])
        self.type = config_dict['type']
        self.result_target_trace = config_dict['target_topic']
        self.result_simulator_trace = 'CA002MatchResultData'
        self.back_file_path = config_dict['back_file_path']
        self.scene_area_5_type_flag = config_dict['type_flag']
        # 背景交通流结束标识
        self.send_data_flag = False

        # redis相关配置项
        self.redis_client = redis.StrictRedis(host='106.120.201.126', port=14611, db=0, password='Wanji@300552!')
        # 冲突区
        self.INTERSECTION_CONFLICT_POLYGON = [(108.900139, 34.374853), (108.900188, 34.374782), (108.900276, 34.374819),
                                              (108.900252, 34.374886)]
        # 全局区域
        self.GLOBAL_LON_LAT_POLYGON = [(108.890857, 34.376314), (108.902723, 34.377569), (108.905985, 34.370729),
                                       (108.891354, 34.370238)]
        # 背景交通流触发区域
        self.TIGGER_CONFLICT_POLYGON = [(108.900493, 34.374973), (108.900496, 34.374934), (108.900553, 34.374950),
                                        (108.900520, 34.374982)]
        # 场景区域1
        self.scene_area_1_intersection_conflict = [(108.900139, 34.374853), (108.900188, 34.374782),
                                                   (108.900276, 34.374819), (108.900252, 34.374886)]
        self.scene_area_1_tigger = [(108.89991933737707, 34.374814766190795), (108.9000194577954, 34.37486307619932),
                                    (108.90009595103294, 34.374706737316984),
                                    (108.89999429029854, 34.374674954360685)]
        self.scene_area_1_send_flag = False
        self.scene_area_1_back_send_flag = False
        self.center_1_lon, self.center_1_lat = calculate_polygon_center(self.scene_area_1_intersection_conflict)
        # 场景区域2
        self.scene_area_2_tigger = [(108.89993191294616, 34.374948072179734), (108.90030174153685, 34.37507322126008),
                                    (108.90034612096798, 34.374969439109464), (108.89997629237735, 34.37486565682934)]
        self.scene_area_2_send_flag = False
        self.scene_area_2_back_send_flag = False
        self.scene_area_2_car_info = dict()
        # self.center_2_lon, self.center_2_lat = calculate_polygon_center(self.scene_area_2_intersection_conflict)
        # 场景区域3
        self.scene_area_3 = [(108.89592475596282, 34.37458197839905), (108.89928837658698, 34.375417094782634),
                             (108.89954816842356, 34.3748791604084), (108.89605693075686, 34.374047800489635)]
        self.scene_area_3_intersection_conflict = [(108.89741647107155, 34.37479845108366),
                                                   (108.89779075407188, 34.374899648604284),
                                                   (108.89787464509016, 34.374750515372554),
                                                   (108.8975035886674, 34.374659970066375)]
        self.scene_area_3_tigger = [(108.89869863020522, 34.37511199154744), (108.89873232516612, 34.37501298635139),
                                    (108.89867976102914, 34.37500631183944), (108.8986528050608, 34.37510420462654)]
        self.scene_area_3_send_flag = False
        self.scene_area_3_back_send_flag = False
        self.center_3_lon, self.center_3_lat = calculate_polygon_center(self.scene_area_3_intersection_conflict)
        # 场景区域4
        self.scene_area_4 = [(108.89451266254605, 34.374621425777704), (108.894853486621, 34.37390988959558),
                             (108.89257197016508, 34.37334396580491), (108.89227525273441, 34.37410183948647)]
        self.scene_area_4_intersection_conflict = [(108.89354222749171, 34.373881373874354),
                                                   (108.8937660601324, 34.37396778679522),
                                                   (108.89382382339511, 34.373821778704894),
                                                   (108.893571109123, 34.373759203731225)]
        self.scene_area_4_tigger = [(108.89406554550288, 34.37402429848957),
                                    (108.8941666303798, 34.374057671434414),
                                    (108.89425019387835, 34.373852983828115),
                                    (108.8941423700108, 34.37383963461967)]
        self.scene_area_4_send_flag = False
        self.scene_area_4_back_send_flag = False
        self.center_4_lon, self.center_4_lat = calculate_polygon_center(self.scene_area_4_intersection_conflict)
        # 场景区域5
        self.scene_area_5_tigger = [(108.89245683191791, 34.37258806261268), (108.89267430449735, 34.37264248495913),
                                    (108.8927101643377, 34.37257946960729), (108.89248343760528, 34.372530775894006)]
        self.scene_area_5_send_flag = False
        self.scene_area_5_back_send_flag = False
        self.scene_area_5_batch = 0

        self.scene_area_5_car_info = dict()
        # self.center_5_lon, self.center_5_lat = calculate_polygon_center(self.scene_area_5_intersection_conflict)
        # 主车所在区域
        self.target_in_area = 0
        self.target_lon = None
        self.target_lat = None
        # 计算多边形中心点的经纬度坐标
        self.center_lon, self.center_lat = calculate_polygon_center(self.INTERSECTION_CONFLICT_POLYGON)
        # 读取目标车辆轨迹
        service_target_thread = threading.Thread(target=self.read_target_traffic_flow)
        service_target_thread.start()
        # 读取背景车辆轨迹
        service_back_thread = threading.Thread(target=self.read_back_traffic_flow)
        service_back_thread.start()

    def read_back_traffic_flow(self):
        print("从车轨迹已就绪")
        while True:
            # 先判断主车位置
            if self.target_in_area == 1:
                if self.scene_area_1_send_flag:
                    with open(r'E:\01文档\00仿真项目\长安大学\实车场景计划\webgl_simulation\file\交叉口处理后.json',
                              'r') as file:
                        for line in file:
                            car_info = json.loads(line)
                            value = car_info['value']
                            for json_data in value:
                                if json_data['id'] == 9999:
                                    continue
                                if json_data['longitude'] == 0 or json_data['longitude'] == '':
                                    continue
                                self.all_car_dict[json_data['id']] = json_data
                            time.sleep(0.1)
                    self.scene_area_1_send_flag = False
                    # 数据读完 清空数据
                    self.all_car_dict = dict()
                    self.start_car_dict = dict()
                else:
                    time.sleep(0.1)
            elif self.target_in_area == 3:
                if self.scene_area_3_send_flag:
                    with open(r'E:\01文档\00仿真项目\长安大学\实车场景计划\webgl_simulation\file\低速汇入处理后.json',
                              'r') as file:
                        for line in file:
                            car_info = json.loads(line)
                            value = car_info['value']
                            for json_data in value:
                                if json_data['id'] == 9999:
                                    continue
                                if json_data['longitude'] == 0 or json_data['longitude'] == '':
                                    continue
                                self.all_car_dict[json_data['id']] = json_data
                            time.sleep(0.1)
                    self.scene_area_3_send_flag = False
                    # 数据读完 清空数据
                    self.all_car_dict = dict()
                    self.start_car_dict = dict()
                else:
                    time.sleep(0.1)
            elif self.target_in_area == 4:
                if self.scene_area_4_send_flag:
                    with open(r'E:\01文档\00仿真项目\长安大学\实车场景计划\webgl_simulation\file\高速汇入处理后.json',
                              'r') as file:
                        for line in file:
                            car_info = json.loads(line)
                            value = car_info['value']
                            for json_data in value:
                                if json_data['id'] == 9999:
                                    continue
                                if json_data['longitude'] == 0 or json_data['longitude'] == '':
                                    continue
                                self.all_car_dict[json_data['id']] = json_data
                            time.sleep(0.1)
                    self.scene_area_4_send_flag = False
                    # 数据读完 清空数据
                    self.all_car_dict = dict()
                    self.start_car_dict = dict()
                else:
                    time.sleep(0.1)
            else:
                # 清空车辆数据
                self.all_car_dict = dict()
                self.start_car_dict = dict()
                time.sleep(0.1)

    def read_target_traffic_flow(self):
        try:
            print("自动驾驶车辆监听已就绪")
            pubsub = self.redis_client.pubsub()
            pubsub.subscribe(self.result_target_trace)
            # 开始监听消息
            for message in pubsub.listen():
                if message['type'] == 'message':
                    receive_data = message['data'].decode('utf-8')
                    data = json.loads(receive_data)
                    json_data = data['value']
                    # print(json_data)
                    for real_car in json_data:
                        id = real_car['id']
                        lon = real_car['longitude']
                        lat = real_car['latitude']
                        point = lon, lat
                        # 如果真车出现在指定区域了 立即发车
                        if point_in_polygon(point, self.scene_area_1_tigger):
                            self.target_in_area = 1
                            self.scene_area_1_send_flag = True
                            self.scene_area_2_send_flag = False
                            self.scene_area_3_send_flag = False
                            self.scene_area_4_send_flag = False
                            self.scene_area_5_send_flag = False
                            self.scene_area_2_car_info = dict()
                            self.scene_area_5_car_info = dict()
                        elif point_in_polygon(point, self.scene_area_2_tigger):
                            self.target_in_area = 2
                            self.scene_area_2_send_flag = True
                            self.scene_area_1_send_flag = False
                            self.scene_area_3_send_flag = False
                            self.scene_area_4_send_flag = False
                            self.scene_area_5_send_flag = False
                            self.scene_area_5_car_info = dict()
                        elif point_in_polygon(point, self.scene_area_3_tigger):
                            self.target_in_area = 3
                            self.target_all_car_dict[id] = real_car
                            self.scene_area_1_send_flag = False
                            self.scene_area_2_send_flag = False
                            self.scene_area_4_send_flag = False
                            self.scene_area_5_send_flag = False
                            self.scene_area_3_send_flag = True
                            self.scene_area_2_car_info = dict()
                            self.scene_area_5_car_info = dict()
                        elif point_in_polygon(point, self.scene_area_4_tigger):
                            self.target_in_area = 4
                            self.target_all_car_dict[id] = real_car
                            self.scene_area_1_send_flag = False
                            self.scene_area_2_send_flag = False
                            self.scene_area_3_send_flag = False
                            self.scene_area_5_send_flag = False
                            self.scene_area_4_send_flag = True
                            self.scene_area_2_car_info = dict()
                            self.scene_area_5_car_info = dict()
                        elif point_in_polygon(point, self.scene_area_5_tigger):
                            self.target_in_area = 5
                            self.target_all_car_dict[id] = real_car
                            self.scene_area_1_send_flag = False
                            self.scene_area_2_send_flag = False
                            self.scene_area_3_send_flag = False
                            self.scene_area_4_send_flag = False
                            self.scene_area_5_send_flag = True
                            self.scene_area_2_car_info = dict()
                        # else:
                        #     self.scene_area_1_send_flag = False
                        #     self.scene_area_2_send_flag = False
                        #     self.scene_area_3_send_flag = False
                        #     self.scene_area_4_send_flag = False
                        #     self.scene_area_5_send_flag = False
                        self.target_all_car_dict[id] = real_car
                        # send_flag = point_in_polygon(point, self.TIGGER_CONFLICT_POLYGON)
                        # if send_flag:
                        #     self.send_data_flag = True
                        # if point_in_polygon(point, self.GLOBAL_LON_LAT_POLYGON):
                        #     self.target_all_car_dict[id] = real_car
        except Exception as e:
            print("读取目标异常", e)

    def ref_beforeStart(self, ref_keepOn):
        iface = tessngIFace()
        # 当前路网名
        tmp_net_path = iface.netInterface().netFilePath()
        if tmp_net_path != self.mNetPath:
            self.mNetPath = tmp_net_path
            self.mSimuCount = 1
        else:
            self.mSimuCount += 1
        # 可在此设置本次仿真参数
        ref_keepOn.value = True

    # 设置本类实现的过载方法被调用频次，即多少个计算周期调用一次。过多的不必要调用会影响运行效率
    def setStepsPerCall(self, vehi):
        # 设置当前车辆及其驾驶行为过载方法被TESSNG调用频次，即多少个计算周调用一次指定方法。如果对运行效率有极高要求，可以精确控制具体车辆或车辆类型及具体场景相关参数
        iface = tessngIFace()
        netface = iface.netInterface()
        netFileName = netface.netFilePath()
        # 允许对车辆重绘方法的调用
        vehi.setIsPermitForVehicleDraw(True)
        # 计算限制车道方法每10个计算周期被调用一次
        vehi.setSteps_calcLimitedLaneNumber(10)
        # 计算安全变道距离方法每10个计算周期被调用一次
        vehi.setSteps_calcChangeLaneSafeDist(10)
        # 重新计算车辆期望速度方法每一个计算周期被调用一次
        vehi.setSteps_reCalcdesirSpeed(1)
        # 重新设置车速方法每一个计算周期被调用一次
        vehi.setSteps_reSetSpeed(1)
        # 范例打开临时路段会会创建车辆方阵，需要进行一些仿真过程控制

        simuface = iface.simuInterface()
        # 仿真精度，即每秒计算次数
        steps = simuface.simuAccuracy()

    # 过载的父类方法， 初始化车辆，在车辆启动上路时被TESS NG调用一次
    def initVehicle(self, vehi):
        # 设置当前车辆及其驾驶行为过载方法被TESSNG调用频次，即多少个计算周调用一次指定方法。如果对运行效率有极高要求，可以精确控制具体车辆或车辆类型及具体场景相关参数
        self.setStepsPerCall(vehi)
        # 车辆ID，不含首位数，首位数与车辆来源有关，如发车点、公交线路
        tmpId = vehi.id() % 100000
        # 车辆所在路段名或连接段名
        roadName = vehi.roadName()
        # 车辆所在路段ID或连接段ID
        roadId = vehi.roadId()
        return True

    # 过载的父类方法重新计算加速度
    def ref_calcAcce(self, vehi, acce):
        return False

    # 过载的父类方法，重新计算期望速度
    # vehi：车辆
    # ref_esirSpeed：返回结果,ref_desirSpeed.value是TESS NG计算好的期望速度，可以在此方法改变它
    # return结果：False：TESS NG忽略此方法作的修改，True：TESS NG采用此方法所作修改
    def ref_reCalcdesirSpeed(self, vehi, ref_desirSpeed):
        # ref_desirSpeed.value = m2p(1)
        # return True
        # print('计算期望速度', vehi.id())
        back_control_dict = self.control_dict.copy()
        all_car_dict = self.all_car_dict.copy()
        start_car_dict = self.start_car_dict.copy()

        if vehi.id() in self.scene_area_5_car_info.keys():
            pos = vehi.pos()
            longitude = pos.x()
            latitude = pos.y()
            lon, lat = self.p(longitude, -latitude, inverse=True)
            if self.target_lon is not None and self.target_lat is not None:
                distance = haversine(lon, lat, self.target_lon, self.target_lat)
                if distance < 10:
                    ref_desirSpeed.value = m2p(0)
                    return True
            ref_desirSpeed.value = m2p(5)
            return True
        pos = vehi.pos()
        longitude = pos.x()
        latitude = pos.y()
        distance = 1000
        lon, lat = self.p(longitude, -latitude, inverse=True)
        if self.target_lon is not None and self.target_lat is not None:
            distance = haversine(lon, lat, self.target_lon, self.target_lat)
        if vehi.id() in self.scene_area_2_car_info.keys():
            if self.type == 1 and distance < 10:
                ref_desirSpeed.value = m2p(20)
                return True
            elif self.type == 2 and distance < 10:
                ref_desirSpeed.value = m2p(1)
                return True
            ref_desirSpeed.value = m2p(5)
            return True

        if vehi.id() not in start_car_dict.keys():
            return False
        if self.type == 1 and distance < 30:
            ref_desirSpeed.value = m2p(20)
            return True
        elif self.type == 2 and distance < 30:
            ref_desirSpeed.value = m2p(1)
            return True
        real_id = start_car_dict.get(vehi.id())
        if real_id in all_car_dict.keys():
            car_info = all_car_dict.get(real_id)
            # print('car_info', self.type, back_control_dict, car_info)
            # 实时调整跟驰参数
            if self.type == 0:
                speed = car_info['speed']
                ref_desirSpeed.value = m2p(speed)
                return True
            elif self.type == 1:
                if real_id in back_control_dict.keys() and back_control_dict.get(real_id):
                    ref_desirSpeed.value = m2p(20)
                    return True
            elif self.type == 2:
                if real_id in back_control_dict.keys() and back_control_dict.get(real_id):
                    ref_desirSpeed.value = m2p(5)
                    return True
            else:
                return False

        return False

    # 过载的父类方法，重新计算跟驰参数：时距及安全距离
    # vehi:车辆
    # ref_inOutSi，安全时距，ref_inOutSi.value是TESS NG已计算好的值，此方法可以改变它
    # ref_inOutSd，安全距离，ref_inOutSd.value是TESS NG已计算好的值，此方法可以改变它
    # return结果：False：TESS NG忽略此方法作的修改，True：TESS NG采用此方法所作修改
    def ref_reSetFollowingParam(self, vehi, ref_inOutSi, ref_inOutSd):
        roadName = vehi.roadName()
        if roadName == "连接段2":
            ref_inOutSd.value = m2p(30)
            return True
        return False

    # 过载的父类方法，重新计算加速度
    # vehi：车辆
    # inOutAce：加速度，inOutAcce.value是TESS NG已计算的车辆加速度，此方法可以改变它
    # return结果：False：TESS NG忽略此方法作的修改，True：TESS NG采用此方法所作修改
    def ref_reSetAcce(self, vehi, inOutAcce):
        roadName = vehi.roadName()
        if roadName == "连接段1":
            if vehi.currSpeed() > m2p(20 / 3.6):
                inOutAcce.value = m2p(-5)
                return True
            elif vehi.currSpeed() > m2p(20 / 3.6):
                inOutAcce.value = m2p(-1)
                return True
        return False

    # 过载的父类方法，重新计算当前速度
    # vehi:车辆
    # ref_inOutSpeed，速度ref_inOutSpeed.value，是已计算好的车辆速度，此方法可以改变它
    # return结果：False：TESS NG忽略此方法作的修改，True：TESS NG采用此方法所作修改
    def ref_reSetSpeed(self, vehi, ref_inOutSpeed):
        # 实时调整跟驰参数
        # if self.type == 0:
        #     print("实时调整车辆期望速度")
        # elif self.type == 1:
        #     print("当车辆到达指定位置后，控制车辆行为为激进")
        # elif self.type == 2:
        #     print("当车辆到达指定冲突区域后，控制车辆行为为保守")
        # else:
        #     print("数据转发")

        return False
        # 将模拟器真车的速度设置为0m/s 通过控制车辆速度，可以实现车辆停车
        # global car_imitate_all_info, car_imitate_start_dict
        # #  如果车辆 是属于模拟器的车辆 则将该车的速度设置为0 通过move方法来控制车辆的位置
        # cId = vehi.id()
        # if cId in car_imitate_start_dict.keys():
        #     ref_inOutSpeed.value = m2p(0)
        #     return True

    # 过载的父类方法，计算是否要左自由变道
    # vehi:车辆
    # return结果，True：变道、False：不变道
    def reCalcToLeftFreely(self, vehi):
        if vehi.id() in self.target_start_car_dict.keys():
            # print(time.time(), vehi.id(), "主车不需要避让", vehi.lane().number())
            return False
        # 前车
        f_vehicle = vehi.vehicleFront()
        if f_vehicle is not None:
            if f_vehicle.id() in self.target_all_car_dict.keys():
                # print(vehi.id, "的前车是主车, 向左变道", fVehicle.id(), "当前所在车道是：", vehi.lane().number())
                # 判断车辆距离在多少米的范围区间内进行变道
                if vehi.vehicleDriving().distToEndpoint() - vehi.length() / 2 < m2p(10):
                    # print(fId, "距离前车", vehi.vehicleDriving().distToEndpoint(), "米，已经无法向左变道")
                    return False
                # 车辆进行变道，判断两个车之间的距离，对车辆进行变道
                if vehi.vehiDistFront() <= m2p(self.avoidDistance):
                    if vehi.lane().number() == 0:
                        # print("车辆满足最左侧车道条件，进行变道")
                        return True
                    if vehi.id() % 2 == 0:
                        return False
                    return True
        # 后车
        # r_vehicle = vehi.vehicleRear()
        # if r_vehicle is not None:
        #     if r_vehicle.id() in self.target_all_car_dict.keys():
        #         # 判断车辆距离在多少米的范围区间内进行变道
        #         if vehi.vehicleDriving().distToEndpoint() - vehi.length() / 2 < m2p(10):
        #             return False
        #         # 车辆进行变道，判断两个车之间的距离，对车辆进行变道
        #         if vehi.vehiDistRear() <= m2p(self.avoidDistance):
        #             if vehi.lane().number() == 0:
        #                 return True
        #             if vehi.id() % 2 == 0:
        #                 return False
        #             return True
        return False

    # 过载的父类方法，计算是否要右自由变道
    # vehi:车辆
    # return结果，True：变道、False：不变道
    def reCalcToRightFreely(self, vehi):
        if vehi.id() in self.target_start_car_dict.keys():
            return False
        # 前车
        f_vehicle = vehi.vehicleFront()
        if f_vehicle is not None:
            if f_vehicle.id() in self.target_all_car_dict.keys():
                # print(vehi.id, "的前车是主车, 向右变道", fVehicle.id(), "当前所在车道是：", vehi.lane().number())
                # 判断车辆距离在多少米的范围区间内进行变道
                if vehi.vehicleDriving().distToEndpoint() - vehi.length() / 2 < m2p(10):
                    # print(fId, "距离前车", vehi.vehicleDriving().distToEndpoint(), "米，已经无法向右变道")
                    return False
                # 车辆进行变道，判断两个车之间的距离，对车辆进行变道
                if vehi.vehiDistFront() <= m2p(self.avoidDistance):
                    if vehi.lane().number() == 2:
                        return True
                    if vehi.id() % 2 != 0:
                        return False
                    return True
        # 后车
        # r_vehicle = vehi.vehicleRear()
        # if r_vehicle is not None:
        #     if r_vehicle.id() in self.target_all_car_dict.keys():
        #         # 判断车辆距离在多少米的范围区间内进行变道
        #         if vehi.vehicleDriving().distToEndpoint() - vehi.length() / 2 < m2p(10):
        #             # print(rId, "距离后车", vehi.vehicleDriving().distToEndpoint(), "米，已经无法向右变道")
        #             return False
        #         # 判断车辆距离在多少米的范围区间内进行变道
        #         if vehi.vehiDistRear() <= m2p(self.avoidDistance):
        #             if vehi.lane().number() == 2:
        #                 return True
        #             if self.event_type == 2:
        #                 self.number = self.number + 1
        #             if vehi.id() % 2 != 0:
        #                 return False
        #             return True
        return False

    # 过载父类方法，设置信号灯色。此例设置ID为5的信号灯色为红色，此方法在每个计算周被调用，所以这个信号灯一直是红色
    # 本次自动创建的路网没有信号灯，在此实现该方法只为说明如何运用
    def calcLampColor(self, signalLamp):
        return False

    # 过载父类方法， 计算车辆当前限制车道序号列表
    def calcLimitedLaneNumber(self, vehi):
        # 如果当前车辆在路段上，且路段ID等于2，则小车走内侧，大车走外侧
        if vehi.roadIsLink():
            # IVehicle.road()方法获取的是车辆当前所路段或连接段的void指针，需要将它转换成路段或连接段
            link = wrapInstance(vehi.road().__int__(), ILink)
            if link is not None and link.id() == 2:
                laneCount = link.laneCount()
                # 小车走内侧，大车走外侧，设长度小于8米为小车
                if (vehi.length() < m2p(8)):
                    return [num for num in range(laneCount // 2 - 1)]
                else:
                    return [num for num in range(laneCount // 2 - 1, laneCount)]
        return []

    # 过载父类方法，车道限速
    def ref_calcSpeedLimitByLane(self, link, laneNumber, ref_outSpeed):
        return False

    # 过载父类方法 对发车点一增加发车时间段
    def calcDynaDispatchParameters(self):
        # TESSNG 顶层接口
        iface = tessngIFace()
        currSimuTime = iface.simuInterface().simuTimeIntervalWithAcceMutiples()
        if currSimuTime % (10 * 1000) == 0 and currSimuTime < 60 * 1000:
            # ID 等于 5 路段上车辆
            lVehi = iface.simuInterface().vehisInLink(47)
            if currSimuTime < 1000 * 30 or len(lVehi) > 0:
                return []
            else:
                now = datetime.now()
                # 当前时间秒
                currSecs = now.hour * 3600 + now.minute * 60 + now.second
                # 仿真 10 秒后且 ID 等于 1 的路段上车辆数为 0，则为 ID 等于 1 的发车点增加发车

                di = Online.DispatchInterval()
                # 动作控制案例-机动车交叉口 L5 路段发车点 ID 为 11
                di.dispatchId = 11
                di.fromTime = currSecs
                di.toTime = di.fromTime + 300 - 1
                di.vehiCount = 300
                di.mlVehicleConsDetail = [Online.VehiComposition(1, 60), Online.VehiComposition(2, 40)]
                return [di]
        return []

    # 在该方法中，通过控制车流比例调整进入道路中的车辆
    # 过载父类方法，动态修改决策点不同路径流量比
    def calcDynaFlowRatioParameters(self):
        return []

    def calcDynaSignalContralParameters(self):
        return []

    # 过载父类方法，停止指定车辆运行，退出路网，但不会从内存删除，会参数各种统计
    #  范例车辆进入ID等于2的路段或连接段，路离终点小于100米，则驰出路网
    def isStopDriving(self, vehi):
        return False

    # 过载的父类方法，TESS NG 在每个计算周期结束后调用此方法，大量用户逻辑在此实现，注意耗时大的计算要尽可能优化，否则影响运行效率
    def afterOneStep(self):
        # try:
        # ===============================全局变量===================================================
        # 首先定义全局变量，用于在不同方法内互相通信
        global car_real_type
        # ===============================车辆顶层接口===============================================
        # TESSNG 顶层接口
        iface = tessngIFace()
        # TESSNG 仿真子接口
        simu_iface = iface.simuInterface()
        # TESSNG 路网子接口
        net_iface = iface.netInterface()
        dispatch_points = net_iface.dispatchPoints()
        batch_num = simu_iface.batchNumber()
        # 当前仿真计算批次
        self.batchNum = batch_num
        # ================================车辆全域数据==============================================
        all_vehicle_start = simu_iface.allVehiStarted()
        # ==================================数据处理===============================================
        # 判断是否超时未发
        timestamp = int(time.time() * 1000)
        current_time = datetime.now().strftime('%Y-%m-%d %H:%M:%S:%f')[:-3]
        # 车辆移除操作
        objects = []
        vehicle_objects = dict()
        for vehicle in all_vehicle_start:
            c_id = vehicle.id()
            # if c_id not in self.start_car_dict.keys():
            #     continue
            speed = vehicle.currSpeed()
            angle = vehicle.angle()
            pos = vehicle.pos()
            acc = vehicle.acce()
            lon = pos.x()
            lat = pos.y()
            v_type = vehicle.vehicleTypeCode()
            longitude, latitude = self.p(lon, -lat, inverse=True)
            # 判断车辆是否在发车列表中
            # 获取车辆真实轨迹
            # 背景车流轨迹输出文件
            if c_id in self.target_start_car_dict.keys():
                car_id =self.target_start_car_dict.get(c_id)
                car_info = self.target_all_car_dict.get(car_id)
                print('主车：',c_id, ',', longitude, ',', latitude, ',速度：', float(car_info['speed']) * 3.6)
                continue
            print(c_id, ',', longitude, ',', latitude, ',速度：', speed * 3.6)
            if c_id in self.start_car_dict.keys():
                if self.start_car_dict[c_id] in self.all_car_dict.keys():
                    back_car = self.all_car_dict.get(self.start_car_dict[c_id])
                    if self.type == 3:
                        acc = back_car['acc']
                        longitude = back_car['longitude']
                        latitude = back_car['latitude']
                        angle = back_car['courseAngle']
                        speed = back_car['speed']
                        v_type = back_car['vehicleType']

            vehicle_json = {
                'timestamp': current_time,
                'globalTimeStamp': timestamp,
                'frameId': batch_num,
                'id': c_id,
                'name': '从车',
                'longitude': longitude,
                'latitude': latitude,
                'courseAngle': angle,
                'vehicleType': v_type,
                'speed': speed * 3.6,
                'acc': acc
            }
            vehicle_objects[c_id] = vehicle_json
            objects.append(vehicle_json)
            # if c_id in self.start_car_dict.keys() and c_id not in self.target_start_car_dict.keys():
            #
            # else:
            #     # 从路网移除车辆
            #     simu_iface.stopVehicleDriving(vehicle)
            # 根据主车所在的区域，以及当前车辆是否在指定区域 移除车辆
            point = longitude, latitude
            if self.target_in_area == 3:
                if not point_in_polygon(point, self.scene_area_3):
                    simu_iface.stopVehicleDriving(vehicle)
            elif self.target_in_area == 4:
                if not point_in_polygon(point, self.scene_area_4):
                    simu_iface.stopVehicleDriving(vehicle)
        send_json = {
            "timestamp": timestamp,
            "timestampType": 'CREATE_TIME',
            "value": objects
        }
        simulator_trace = json.dumps(send_json)
        self.redis_client.publish(self.result_simulator_trace, simulator_trace)
        # ==============================发车逻辑处理===========================================
        if self.scene_area_1_send_flag or self.scene_area_3_send_flag or self.scene_area_4_send_flag:
            # 背景交通流处理逻辑 控制轨迹是move 还是通过轨迹移动
            all_car_dict = self.all_car_dict.copy()
            start_car_dict = self.start_car_dict.copy()
            for carId in all_car_dict:
                car = all_car_dict[carId]
                longitude = car['longitude']
                latitude = car['latitude']
                speed = car['speed']
                # 获取到车辆信息
                if find_key_by_value(start_car_dict, carId) is not None:
                    car_id = find_key_by_value(start_car_dict, carId)
                    vehicle = simu_iface.getVehicle(car_id)
                    # 判断当前驾驶行为类型 0-tess托管 1-激进型 2-保守型 3-只转发
                    if self.type == 0:
                        continue
                    elif self.type == 3:
                        # 认定该车辆已经存在于发车列表中，此时更新该车辆的位置
                        if vehicle is not None:
                            # 获取驾驶行为
                            lon, lat = self.p(longitude, latitude, inverse=False)
                            vehicle_driving = vehicle.vehicleDriving()
                            q_point = QPointF()
                            q_point.setX(lon)
                            q_point.setY(-lat)
                            # 获取离目标点最近的坐标，共9个单元格，一般 lLocation是最近的
                            l_location = net_iface.locateOnCrid(q_point, 9)
                            if len(l_location) > 0:
                                vehicle_driving.move(l_location[0].pLaneObject, l_location[0].distToStart)
                            else:
                                print('未找到该车辆的位置信息')
                            continue
                    else:
                        # 根据区域判断轨迹
                        tar_t = 100
                        if car_id in vehicle_objects.keys():
                            car_info = vehicle_objects[car_id]
                            # 激进车
                            if car_id in self.control_dict.keys():
                                continue
                            target_all_car_copy = self.target_all_car_dict.copy()
                            if self.target_in_area == 1:
                                center_lon = self.center_1_lon
                                center_lat = self.center_1_lat
                            elif self.target_in_area == 3:
                                center_lon = self.center_3_lon
                                center_lat = self.center_3_lat
                            elif self.target_in_area == 4:
                                center_lon = self.center_4_lon
                                center_lat = self.center_4_lat
                            else:
                                continue
                            for tar_key in target_all_car_copy.keys():
                                target_car_info = target_all_car_copy.get(tar_key)
                                longitude = target_car_info['longitude']
                                latitude = target_car_info['latitude']
                                # 计算经纬度距离中心点的距离
                                # 计算待判断点到多边形中心点的距离
                                distance = haversine(longitude, latitude, center_lon, center_lat)
                                if target_car_info['speed'] == 0 or target_car_info['speed'] == '':
                                    continue
                                tar_t = distance / float(target_car_info['speed'])
                            # 判断该背景车距离 冲突点的距离
                            # 根据当前时间计算背景车和主车到达冲突点的时间
                            if speed == 0:
                                continue
                            back_distance = haversine(float(car_info['longitude']), float(car_info['latitude']),
                                                      float(center_lon), float(center_lat))
                            back_t = back_distance / speed
                            # 背景车先到，主车后到
                            if tar_t - back_t < 1 and back_t - tar_t < 1:
                                # 需要做发车控制 终点
                                print('需要做应急预警控制', tar_t - back_t)
                                random_int = random.randint(1, 100)
                                if random_int < self.radical_ratio:
                                    self.radical_car_list.append(car_id)
                                else:
                                    self.conservative_car_list.append(car_id)
                            # 判断
                    continue
                lon, lat = self.p(longitude, latitude, inverse=False)
                q_point = QPointF()
                q_point.setX(lon)
                q_point.setY(-lat)
                # 获取离目标点最近的坐标，共9个单元格，一般 lLocation是最近的
                l_location = net_iface.locateOnCrid(q_point, 9)
                # 判断获取到最近的坐标集合是否为空
                if len(l_location) > 0:
                    p_lane_object = l_location[0].pLaneObject
                    # 路段发车
                    if isinstance(p_lane_object, ILane):
                        # 发车信息初始化
                        dvp = Online.DynaVehiParam()
                        dvp.vehiTypeCode = car_real_type[car['vehicleType']]
                        dvp.laneNumber = p_lane_object.number()
                        p_link = p_lane_object.link()
                        dvp.roadId = p_link.id()
                        dvp.dist = l_location[0].distToStart
                        dvp.speed = speed
                        dvp.color = f"#FFFFFF"
                        vehicle = simu_iface.createGVehicle(dvp, True)
                        if vehicle is not None:
                            self.start_car_dict[vehicle.id()] = carId
                        else:
                            print('路段车辆创建失败')
                    # 连接段发车
                    else:
                        # 发车信息初始化
                        dvp = Online.DynaVehiParam()
                        dvp.vehiTypeCode = car_real_type[car['vehicleType']]
                        # 获取连接段
                        connector = p_lane_object.connector()
                        # 根据车辆类型在指定车道发车（机动车仅能在机动车道行驶，非机动车(5,6,7)则能在所有车道行驶） 目前能够精确到具体车道
                        from_link = connector.fromLink()
                        dvp.laneNumber = random.randint(1, 3)
                        # 这里理论上需要计算发车点距离路段起点的位置
                        # （会首先获取当前车道所有的起点位置，计算起点位置距离车辆经纬度的距离，作为发车点距离路段起点的距离）
                        dvp.roadId = from_link.id()
                        dvp.dist = from_link.length() - 1
                        dvp.speed = speed
                        dvp.color = f"#FFFFFF"
                        vehicle = simu_iface.createGVehicle(dvp, True)
                        if vehicle is not None:
                            self.start_car_dict[vehicle.id()] = carId
                        else:
                            print('连接段车辆创建失败')
        elif self.scene_area_2_send_flag:
            new_batch_num = 0
            scene_area_2_car_info = self.scene_area_2_car_info.copy()
            if len(scene_area_2_car_info) < 3:
                for key in scene_area_2_car_info:
                    v_batch_num = scene_area_2_car_info.get(key)
                    if new_batch_num < int(v_batch_num):
                        new_batch_num = int(v_batch_num)
                # 触发发车 道路上没有车或者满足小于2辆，且发车间隔大于2秒
                if len(scene_area_2_car_info) == 1 or (batch_num - new_batch_num) > 20:
                    dvp = Online.DynaVehiParam()
                    dvp.vehiTypeCode = 1
                    dvp.laneNumber = 0
                    dvp.roadId = 413
                    dvp.dist = 0
                    dvp.speed = 10
                    dvp.color = f"#FFFFFF"
                    vehicle = simu_iface.createGVehicle(dvp, True)
                    if vehicle is not None:
                        self.scene_area_2_car_info[vehicle.id()] = batch_num
                    else:
                        print('路段车辆创建失败')
                if len(scene_area_2_car_info) == 2 or (batch_num - new_batch_num) > 20:
                    dvp = Online.DynaVehiParam()
                    dvp.vehiTypeCode = 1
                    dvp.laneNumber = 0
                    dvp.roadId = 63
                    dvp.dist = 0
                    dvp.speed = 10
                    dvp.color = f"#FFFFFF"
                    vehicle = simu_iface.createGVehicle(dvp, True)
                    if vehicle is not None:
                        self.scene_area_2_car_info[vehicle.id()] = batch_num
                    else:
                        print('路段车辆创建失败')

        elif self.scene_area_5_send_flag:
            # 行人闯入
            if self.scene_area_5_type_flag == 0:
                if (batch_num - self.scene_area_5_batch) > 100:
                    dvp = Online.DynaVehiParam()
                    dvp.vehiTypeCode = 7
                    dvp.laneNumber = 0
                    dvp.roadId = 580
                    dvp.dist = 0
                    dvp.speed = 0
                    dvp.color = f"#FFFFFF"
                    vehicle = simu_iface.createGVehicle(dvp, True)
                    if vehicle is not None:
                        self.scene_area_2_car_info[vehicle.id()] = batch_num
                    else:
                        print('行人创建失败')
                    self.scene_area_5_batch = batch_num
            # 紧急停车
            elif self.scene_area_5_type_flag == 1:
                if len(self.scene_area_5_car_info) < 1:
                    dvp = Online.DynaVehiParam()
                    dvp.vehiTypeCode = 1
                    dvp.laneNumber = 0
                    dvp.roadId = 574
                    dvp.dist = 0
                    dvp.speed = 0
                    dvp.color = f"#FFFFFF"
                    vehicle = simu_iface.createGVehicle(dvp, True)
                    if vehicle is not None:
                        self.scene_area_5_car_info[vehicle.id()] = batch_num
                    else:
                        print('路段车辆创建失败')
        # 模拟器真车处理逻辑
        target_all_car_dict = self.target_all_car_dict
        target_start_car_dict = self.target_start_car_dict
        for carId in target_all_car_dict.keys():
            car = target_all_car_dict[carId]
            # print(car)
            longitude = car['longitude']
            latitude = car['latitude']
            self.target_lon = longitude
            self.target_lat = latitude
            lon, lat = self.p(longitude, latitude, inverse=False)
            if find_key_by_value(target_start_car_dict, carId) is not None:
                # 首先判断该车辆是否在发车列表中
                if carId in target_start_car_dict.values():
                    car_id = find_key_by_value(target_start_car_dict, carId)
                    # 认定该车辆已经存在于发车列表中，此时更新该车辆的位置
                    vehicle = simu_iface.getVehicle(car_id)
                    if vehicle is not None:
                        # 获取驾驶行为
                        vehicle_driving = vehicle.vehicleDriving()
                        q_point = QPointF()
                        q_point.setX(lon)
                        q_point.setY(-lat)
                        # 获取离目标点最近的坐标，共9个单元格，一般 lLocation是最近的
                        l_location = net_iface.locateOnCrid(q_point, 9)
                        if len(l_location) > 0:
                            vehicle_driving.move(l_location[0].pLaneObject, l_location[0].distToStart)
                        else:
                            print('未找到该车辆的位置信息')
                        continue
            else:
                # 进行发车操作
                q_point = QPointF()
                q_point.setX(lon)
                q_point.setY(-lat)
                # 获取离目标点最近的坐标，共9个单元格，一般 lLocation是最近的
                l_location = net_iface.locateOnCrid(q_point, 9)
                # 判断获取到最近的坐标集合是否为空
                if len(l_location) > 0:
                    p_lane_object = l_location[0].pLaneObject
                    # 路段发车
                    if isinstance(p_lane_object, ILane):
                        l_link = p_lane_object.link()
                        ll_id = l_link.id()
                        sec_from_connect_link = l_link.fromConnectors()
                        if len(sec_from_connect_link) > 0:
                            iConnector = sec_from_connect_link[0]
                            lLink = iConnector.fromLink()
                            ll_id = lLink.id()
                            print('上游道路id', lLink.id())
                        # 发车信息初始化
                        dvp = Online.DynaVehiParam()
                        dvp.vehiTypeCode = car_real_type[car['vehicleType']]
                        # 根据车辆类型在指定车道发车（机动车仅能在机动车道行驶，非机动车(5,6,7)则能在所有车道行驶） 目前能够精确到具体车道
                        dvp.laneNumber = p_lane_object.number()
                        # 这里理论上需要计算发车点距离路段起点的位置
                        # （会首先获取当前车道所有的起点位置，计算起点位置距离车辆经纬度的距离，作为发车点距离路段起点的距离）
                        dvp.roadId = l_link.id()
                        dvp.dist = l_location[0].distToStart
                        dvp.speed = 0
                        dvp.color = f"#4169E1"
                        print('主车所在道路ID:', dvp.roadId, '所在车道', dvp.laneNumber)
                        for vehicle in all_vehicle_start:
                            v_road = vehicle.roadId()
                            v_lane = vehicle.lane()
                            if dvp.roadId == v_road and dvp.laneNumber == v_lane.number():
                                print('主车所在道路ID:', dvp.roadId, '所在车道', dvp.laneNumber,
                                      '移除所在道路ID:', v_road, '所在车道', v_lane.number())
                                simu_iface.stopVehicleDriving(vehicle)
                            if dvp.roadId == ll_id and dvp.laneNumber == v_lane.number():
                                print('主车所在道路ID:', dvp.roadId, '所在车道', dvp.laneNumber,
                                      '移除所在道路ID:', v_road, '所在车道', v_lane.number())
                                simu_iface.stopVehicleDriving(vehicle)

                        vehicle = simu_iface.createGVehicle(dvp, True)
                        if vehicle is not None:
                            self.target_start_car_dict[vehicle.id()] = carId
                        else:
                            # pass
                            print('路段车辆创建失败')

                    # 连接段发车
                    else:
                        # 发车信息初始化
                        dvp = Online.DynaVehiParam()
                        dvp.vehiTypeCode = car_real_type[car['vehicleType']]
                        # 获取连接段
                        connector = p_lane_object.connector()
                        # 根据车辆类型在指定车道发车（机动车仅能在机动车道行驶，非机动车(5,6,7)则能在所有车道行驶） 目前能够精确到具体车道
                        from_link = connector.fromLink()
                        dvp.laneNumber = random.randint(1, 2)
                        # 这里理论上需要计算发车点距离路段起点的位置
                        # （会首先获取当前车道所有的起点位置，计算起点位置距离车辆经纬度的距离，作为发车点距离路段起点的距离）
                        dvp.roadId = from_link.id()
                        dvp.dist = from_link.length() - 1
                        dvp.speed = 0
                        dvp.color = f"#4169E1"
                        vehicle = simu_iface.createGVehicle(dvp, True)
                        if vehicle is not None:
                            self.target_start_car_dict[vehicle.id()] = carId
                        else:
                            pass


def find_key_by_value(dictionary, value):
    for key, val in dictionary.items():
        if val == value:
            return key
    return None
